
#include <math.h>
#include <stdio.h>

#include "globals.h"
#include "geometry.h"
#include "polygon.h"
#include "voronoi.h"
#include "distance.h"
#include "sgidrawi.h"
#include "sweep.h"

void
ptNearestPgonLine(Pt *point, Pgon *line, int startindex, Pt *nearest)
{
	Line	original;

	LineOf(&(line->points[startindex]), &(line->points[
		(startindex+1)%line->numPoints]), &original);
	ptNearestLine (point, &original, nearest);
}

boolean
nearestPointOnSegment(Pt *queryPt, Pt *seg1, Pt *seg2)
{
	Line	segment, normal1, normal2;
	Pt		testPoint;

	if (PointAtInfinity(*queryPt))
		if (PointAtInfinity(*seg1) || PointAtInfinity(*seg2))
			return (True);
		else
			return (False);

	PointAssign(testPoint, *queryPt);
	if (testPoint.w < 0) {
		testPoint.w *= -1;
		testPoint.x *= -1;
		testPoint.y *= -1;
	}

	LineOf(seg1, seg2, &segment);
	perpRightTurnLine(seg1, &segment, &normal1);
	perpRightTurnLine(seg2, &segment, &normal2);

	if ((LineSide (&normal1, &testPoint) == LeftSide) &&
		(LineSide (&normal2, &testPoint) == RightSide))
		return (True);
	else
		return (False);
}

void
spokeToSegment (Pt *origin, Pgon *object, int index1, int index2, 
	spokeInfo *spoke)
{
	Real		dist0, dist1;
	Line		compareLine;

	if (index1 == index2) {
		PointAssign(spoke->attachment, object->points[index1]);
		spoke->attachIndex = 2*index1;
	}
	else {
		if (nearestPointOnSegment(origin, &(object->points[index1]),
				&(object->points[index2])) == True) {
			ptNearestPgonLine(origin, object, index1, &(spoke->attachment));
			LineOf(&(object->points[index1]), &(object->points[index2]),
				&compareLine);
			if (LineSide(&compareLine, origin) == RightSide) {
				spoke->attachIndex = 2*index1 + 1;
			}
			else {
				spoke->attachIndex = 2*index2 + 1;
			}
		}
		else {
			squarePointDistance(origin, &(object->points[index1]), &dist0);
			squarePointDistance(origin, &(object->points[index2]), &dist1);

			if (Less(dist0, dist1)) {
				PointAssign(spoke->attachment, object->points[index1]);
				spoke->attachIndex = 2*index1;
			}
			else {
				PointAssign(spoke->attachment, object->points[index2]);
				spoke->attachIndex = 2*index2;
			}
		}
	}
}

void
findSpoke(Pt *origin, Pgon *object, spokeInfo *spoke)
{
	int			bounds[4], swap, i, j;
	Real		dist, farthest;
	int			start, end, mid, next;
	Line		testline;

	/*	The first point of the spoke can be our origin itself. */

	PointAssignPtr(&(spoke->otherPt), origin);
	spoke->attachPolygon = object;

	if (object->numPoints == 1)
		start = end = 0;
	else if (object->numPoints == 2) {
		start = 0;
		end = 1;
	}
	else {
		Line	pgonLine, testNorm;
		int		diff;

		/*	Find myself a starting point.  I want a point on the polygon
			that is on the other side from the query point. */

		end = object->leftmost;
		start = (end + 1) % object->numPoints;
		LineOf(&(object->points[end]), &(object->points[start]), &pgonLine);
		if ((LineSide(&pgonLine, origin) == RightSide) ||
				(OnTheLine(&pgonLine, origin))) {
			end = object->rightmost;
			start = (end + 1) % object->numPoints;
			LineOf(&(object->points[end]), &(object->points[start]), &pgonLine);
			if ((LineSide(&pgonLine, origin) == RightSide) ||
					(OnTheLine(&pgonLine, origin))) {
				end = object->upper;
				start = (end + 1) % object->numPoints;
				LineOf(&(object->points[end]), &(object->points[start]), 
					&pgonLine);
				if ((LineSide(&pgonLine, origin) == RightSide) ||
						(OnTheLine(&pgonLine, origin))) {
					/*	We've got a really messed up polygon here. */
					end = object->lower;
					start = (end + 1) % object->numPoints;
				}
			}
		}

		/*	Do a binary search across the polygon using the normals
			to the edges of the polygon. */

		while (end != ((start + 1)% object->numPoints)) {
			diff = (end - start + object->numPoints) % object->numPoints;
			mid = (start + (int)(diff / 2)) % object->numPoints;
			LineOf(&(object->points[mid]), 
				&(object->points[(mid+1)%object->numPoints]), 
				&pgonLine);
			perpRightTurnLine(&(object->points[mid]), &pgonLine, &testNorm);
			if (LineSide(&testNorm, origin) == LeftSide)
				start = mid;
			else
				end = mid;
		}
	}
	
	spokeToSegment (origin, object, start, end, spoke);
}

void
ptNearestPgon(Pt *query, Pgon *poly, Pt *nearest)
{
    spokeInfo   spoke;

    findSpoke(query, poly, &spoke);
    PointAssignPtr(nearest, &(spoke.attachment));
}

/*	Uses the points (a,b,c) and (d,e,f) in homogeneous coordinates. */

void
pointBisector(Pt *p1, Pt *p2, Line *l)
{
	double	cd, af, bf, ce, bd, ae, ad;

	/*	Taking the bisector of (a,b,c) with (d,e,f) */

	cd = (double) p1->y * p2->w;
	af = (double) p1->w * p2->y;
	bf = (double) p1->x * p2->y;
	ce = (double) p1->y * p2->x;
	bd = (double) p1->x * p2->w;
	ae = (double) p1->w * p2->x;
	ad = (double) p1->w * p2->w;

	/*	With these pre-processed values, create the bisector */

	l->w = (Coordinate)((ae*ae) - (bd*bd) - (cd*cd) + (af*af));
	l->x = (Coordinate)(2.0*ad*(bd - ae));
	l->y = (Coordinate)(2.0*ad*(cd - af));
}

void
lineBisectors(Line *ln1, Line *ln2, Line *bisect1, Line *bisect2)
{
	Pt		intPt, otherPt1, otherPt2;

	IntersectionOf(ln1, ln2, &intPt);

	if (LineSlopePtr(ln1) == LineSlopePtr(ln2)) {
		/*	Parallel lines -- only one bisector for these. */

		bisect1->w = ln1->w * ln2->y + ln1->y * ln2->w;
		bisect1->x = 2 * ln1->x * ln2->y;
		bisect1->y = 2 * ln1->y * ln2->y;

		PointAssignPtr(bisect2, bisect1);
	}
	else if (((ln1->x == 0) && (ln2->y == 0)) || 
			 ((ln1->y == 0) && (ln2->x == 0))) {

		/*	The lines are both parallel to different coordinate axes. */

		PointAssign(otherPt1, intPt);
		PointAssign(otherPt2, intPt);

		otherPt1.x += otherPt1.w;
		otherPt1.y += otherPt1.w;

		otherPt2.x -= otherPt2.w;
		otherPt2.y += otherPt2.w;

		LineOf(&intPt, &otherPt1, bisect1);
		LineOf(&intPt, &otherPt2, bisect2);
	}
	else {

		double sign, len1, len2;
		Pt midp;

#define SGN(x) (x < 0 ? -1 : 1)
		 
		sign = SGN(intPt.w);
		len1 =   sign * sqrt(ln1->x * ln1->x + ln1->y * ln1->y);
		len2 = - sign * sqrt(ln2->x * ln2->x + ln2->y * ln2->y);

		midp.w = 0.0;
		midp.x = ln1->x / len1 + ln2->x / len2;
		midp.y = ln1->y / len1 + ln2->y / len2;

		LineOf(&intPt, &midp, bisect1);
		PointAssignPtr(bisect2, bisect1);
		perpendicularLine(&intPt, bisect1, bisect2);
	}
}

#define DifferentPoints(A,B,C) !(\
	SamePoint(A,B) || SamePoint(A,C) || SamePoint(B,C))

boolean
pointsCCW(Pt *a, Pt *b, Pt *c)
{
	return((Greater(Det3(a,b,c), 0) ? True : False) || !DifferentPoints(a,b,c));
}

boolean
pointsCCWAllowSame(Pt *a, Pt *b, Pt *c)
{
	return((Greater(Det3(a,b,c), 0) ? True : False) || DifferentPoints(a,b,c));
}


boolean
objectsOnHull(Pt *pt1, Pt *pt21, Pt *pt22, Pt *pt3, Pt *vorPt, Pt *eventPt, 
	spokeInfo *spoke)
{
	Pt			vect1, vect2;
	Pt			resultant;
	Line		tangent;
	Real		len1, len2;
	int			direction;

	PointAssign(spoke->attachment, *pt21);
	spoke->attachIndex = -1;		/*	Don't care about this now.  */
	LineOf(pt1, pt21, &tangent);
	if ((OnTheLine(&tangent, pt22) == True) && 
		(OnTheLine(&tangent, pt3) == True)) {
		/*	The tangents are collinear! */
		if (YValPtr(pt1) != YValPtr(pt21)) {
			if (YValPtr(pt1) > YValPtr(pt21))
				direction = 1;
			else
				direction = -1;

			(spoke->otherPt).w = pt21->w * tangent.x;
			(spoke->otherPt).x = tangent.x *
			(pt21->x + pt21->w * direction);
			(spoke->otherPt).y = pt21->y * tangent.x +
				direction * pt21->w * tangent.y;
		}
		else {
			(spoke->otherPt).w = pt21->w;
			(spoke->otherPt).x = pt21->x;
			if (XValPtr(pt1) < XValPtr(pt21))
				(spoke->otherPt).y = pt21->y + 1;
			else
				(spoke->otherPt).y = pt21->y - 1;
		}
	}
	else {
		/*	Give up on elegance -- use some square roots for now. */

		SubPoints(*pt21, *pt1, vect1);
		SubPoints(*pt22, *pt3, vect2);
		vectorLength(&vect1, &len1);
		vectorLength(&vect2, &len2);
		vect1.w *= len1;
		vect2.w *= len2;
		AddPoints(vect1, vect2, resultant);
		AddPoints(*pt21, resultant, spoke->otherPt);
	}

	/*	The event point is at infinity. */

	eventPt->w = 0;
	eventPt->x = 1.0;
	eventPt->y = 0;

	vorPt->w = 0;
	vorPt->x = 0.0;
	vorPt->y = 0;

	return (False);
}


void
getEventPoint (Pt *vorPoint, Pt *attach, Pt *eventPt)
{
	Real	spokeLength;

	PointAssignPtr(eventPt, vorPoint);
	pointDistance(attach, vorPoint, &spokeLength);
	eventPt->x += eventPt->w * spokeLength;
}

/*	Assert that the points pointed to by the entries of startIdx in
	the correspoinding pgonList are within the polygon and that the
	points appear in the array offset order of 0, 1, then 2
	counterclockwise around the Voronoi vertex. */

boolean
pppVoronoi(Pgon **pgonList, int *startIdx, Pt *vorPoint, Pt *eventPt,
	spokeInfo *middleSpoke)
{
	Line	bisect1, bisect2;
	Pt		*p1, *p2, *p3;
	boolean	ccw, ignore;

	p1 = &(pgonList[0]->points[startIdx[0]]);
	p2 = &(pgonList[1]->points[startIdx[1]]);
	p3 = &(pgonList[2]->points[startIdx[2]]);

	ccw = pointsCCW(p1, p2, p3);

	if (ccw == True) {
		pointBisector(p1, p2, &bisect1);
		pointBisector(p1, p3, &bisect2);

		IntersectionOf(&bisect1, &bisect2, vorPoint);

		/*	Figure out where the circle event will happen. */

		getEventPoint (vorPoint, p1, eventPt);

		/*	Set up the middle spoke at the same time. */

		middleSpoke->attachPolygon = pgonList[1];
		PointAssignPtr(&(middleSpoke->otherPt), vorPoint);
		PointAssignPtr(&(middleSpoke->attachment), p2);
		middleSpoke->attachIndex = 2*startIdx[1];
	}
	else {
		ignore = objectsOnHull(p1, p2, p2, p3, vorPoint, eventPt, middleSpoke);
	}

	return (ccw);
}

#ifndef Jack
/*	Assert at the start that pgonList[2] is the line and the polygons
	appear in the counterclockwise order or array offsets 0, 1, then 2.  
	Such an order can always be found before entering this routine. */

boolean
pplVoronoi(Pgon **pgonList, int *startIdx, int lineIdx, Pt *vorPoint, 
	Pt *eventPt, spokeInfo *middleSpoke)
{
	Pt			candidate1, candidate2, nearPt1, nearPt2;
	Pt			*pt1, *pt2, *segPt1, *segPt2;
	Line		bisect, directrix, compareline;
	Parabola	para;
	boolean		ccw1, ccw2, ignore;

	pt1 = &(pgonList[(lineIdx+1)%3]->points[startIdx[(lineIdx+1)%3]]);
	pt2 = &(pgonList[(lineIdx+2)%3]->points[startIdx[(lineIdx+2)%3]]);
	segPt1 = &(pgonList[lineIdx]->points[startIdx[lineIdx]]);
	segPt2 = &(pgonList[lineIdx]->points[(startIdx[lineIdx] + 1) % 
				pgonList[lineIdx]->numPoints]);

	/*	The two points are bisected by a straight line. */

	pointBisector(pt1, pt2, &bisect);

	/*	Choose one point and the line itself to form a bisecting parabola. */

	LineOf(segPt1, segPt2, &directrix);

	createParabola(pt1, &directrix, &para);

	/*	Since we are only playing with three objects, there must be
		an intersection between the line and the parabola. */

	intersectLineParabola(&bisect, &para, &candidate1, &candidate2);

	/*	With my degeneracy test set of points, a configuration like
		90 80, 100 75, 100 80, 100 85 would give the wrong answer.
		The problem was that nearPt1 and nearPt2 were being calculated
		off by about 7x10^-7 and were failing the ccw test (when
		pt1 = 100 85, pt2 = 100 80.  The solution for now is to round
		the candidate points so their error doesn't propagate to 
		nearPt1 or nearPt2. */

	RoundPoint(candidate1);
	RoundPoint(candidate2);

	/*	Choose the one that sets the polygons in the correct order. */
	/*	The compare line takes the two points in ccw order. */

	LineOf(pt1, pt2, &compareline); 
	ptNearestLine(&candidate1, &directrix, &nearPt1);
	ptNearestLine(&candidate2, &directrix, &nearPt2);

	ccw1 = pointsCCW(pt1, pt2, &nearPt1);
	ccw2 = pointsCCW(pt1, pt2, &nearPt2);

	if (ccw1 || ccw2 || OnTheLine(&directrix, pt1) || 
		OnTheLine(&directrix, pt2)) {
		middleSpoke->attachPolygon = pgonList[1];
		middleSpoke->attachIndex = 2*startIdx[1];
			if (lineIdx == 1) 
				middleSpoke->attachIndex += 1;

		if (ccw1) {
			PointAssignPtr(vorPoint, &candidate1);
			PointAssign(middleSpoke->attachment, nearPt1);
			getEventPoint (vorPoint, &nearPt1, eventPt);
		}
		else{
			PointAssignPtr(vorPoint, &candidate2);
			PointAssign(middleSpoke->attachment, nearPt2);
			getEventPoint (vorPoint, &nearPt2, eventPt);
		}
		PointAssignPtr(&(middleSpoke->otherPt), vorPoint);
	}
	else {
		Pt	tan1Start, tan1End, tan2Start, tan2End;
		int	headIdx, tailIdx;

        /*  Compute the spoke as if on the convex hull.  Set the
            event point at infinity. */

        outerCommonTangent(pgonList[0], pgonList[1], &tan1Start, &tan1End,
			&headIdx, &tailIdx);
        outerCommonTangent(pgonList[1], pgonList[2], &tan2Start, &tan2End,
			&headIdx, &tailIdx);

        ignore = objectsOnHull(&tan1Start, &tan1End, &tan2Start, &tan2End,
                vorPoint, eventPt, middleSpoke);
	}

	return (ccw1 || ccw2);
}

/*	Assert at the start that pgonList[0] is the point and the polygons
	appear in the counterclockwise order or array offsets 0, 1, then 2.  
	Such an order can always be found before entering this routine. */

boolean
pllVoronoi(Pgon **pgonList, int *startIdx, int ptIdx, Pt *vorPoint, 
		Pt *eventPt, spokeInfo *middleSpoke)
{
	Pt			candidate[4], nearPt[2];
	Pt			*ptSite, *seg11, *seg12, *seg21, *seg22;
	Line		bisect[2], lines[2];
	Parabola	para;
	int			i, j;
	boolean		done, ignore;

	ptSite = &(pgonList[ptIdx]->points[startIdx[ptIdx]]);
	seg11 = &(pgonList[(ptIdx+1)%3]->points[startIdx[(ptIdx+1)%3]]);
	seg12 = &(pgonList[(ptIdx+1)%3]->points[(startIdx[(ptIdx+1)%3] + 1) % 
				pgonList[(ptIdx+1)%3]->numPoints]);
	seg21 = &(pgonList[(ptIdx+2)%3]->points[startIdx[(ptIdx+2)%3]]);
	seg22 = &(pgonList[(ptIdx+2)%3]->points[(startIdx[(ptIdx+2)%3] + 1) % 
				pgonList[(ptIdx+2)%3]->numPoints]);

	/*	The two lines are bisected by a straight line.  Of course, to
		muddle things up, there are two such bisectors. */

	LineOf(seg11, seg12, &(lines[0]));
	LineOf(seg21, seg22, &(lines[1]));
	lineBisectors(&(lines[0]), &(lines[1]), &(bisect[0]), &(bisect[1]));

	/*	The one point and either line define a bisecting parabola. */

	createParabola(ptSite, &(lines[0]), &para);

	/*	Since we are only playing with three objects, there must be
		an intersection between the line and the parabola. */

	intersectLineParabola(&(bisect[0]), &para, &(candidate[0]), &candidate[1]);
	intersectLineParabola(&(bisect[1]), &para, &(candidate[2]), &candidate[3]);

	/*	Choose the one that sets the polygons in the correct order. */
	/*	The compare line takes the two points in ccw order. */

	done = False;
	for (i = 0; (i < 4) && (done == False); i++) {
		for (j = 0; j < 2; j++) {
			ptNearestLine(&(candidate[i]), &(lines[j]), &(nearPt[j]));
		}

		if (pointsCCW(ptSite, &(nearPt[0]), &(nearPt[1])) == True) {
			if ((nearestPointOnSegment(&(candidate[i]), seg11, seg12) == True)&&
				(nearestPointOnSegment(&(candidate[i]), seg11, seg12) == True)){
				PointAssignPtr(vorPoint, &(candidate[i]));
				done = True;
			}
		}
	}

	if (done) {
		getEventPoint (vorPoint, ptSite, eventPt);

		middleSpoke->attachPolygon = pgonList[1];
		PointAssignPtr(&(middleSpoke->otherPt), vorPoint);
		ptNearestLine(vorPoint, &(lines[0]), &(middleSpoke->attachment));
		middleSpoke->attachIndex = 2*startIdx[1];
		if (ptIdx != 1)
			middleSpoke->attachIndex += 1;
	}
	else {
		Pt	tan1Start, tan1End, tan2Start, tan2End;
		int	headIdx, tailIdx;

        /*  Compute the spoke as if on the convex hull.  Set the
            event point at infinity. */

        outerCommonTangent(pgonList[0], pgonList[1], &tan1Start, &tan1End,
			&headIdx, &tailIdx);
        outerCommonTangent(pgonList[1], pgonList[2], &tan2Start, &tan2End,
			&headIdx, &tailIdx);

        ignore = objectsOnHull(&tan1Start, &tan1End, &tan2Start, &tan2End,
                vorPoint, eventPt, middleSpoke);
	}

	return (done);
}
#endif

/*	Assert that the lines pointed to by the entries of startIdx in
	the correspoinding pgonList are within the polygon and that the
	lines appear in the array offset order of 0, 1, then 2
	counterclockwise around the Voronoi vertex.  Although we can
	get up to three ccw Voronoi vertices, the routine selects the
	one ccw vertex that is adjacent to each of the segments
	themselves.  Since the segments cannot intersect, only one
	of the vertices can satisfy this requirement. */

boolean
lllVoronoi(Pgon **pgonList, int *startIdx, Pt *vorPoint, Pt *eventPt, 
		spokeInfo *middleSpoke)
{
	Pt			candidate[4];
	Line		bisect[2][2], lines[3];
	int			i, j;
	Pt			nearPt[3];
	boolean		done, ignore;

	/*	The two lines are bisected by a straight line.  Of course, to
		muddle things up, there are two such bisectors. */

	LineOf(&(pgonList[0]->points[startIdx[0]]), 
		&(pgonList[0]->points[(startIdx[0] + 1) % pgonList[0]->numPoints]), 
		&(lines[0]));
	LineOf(&(pgonList[1]->points[startIdx[1]]), 
		&(pgonList[1]->points[(startIdx[1] + 1) % pgonList[1]->numPoints]), 
		&(lines[1]));
	LineOf(&(pgonList[2]->points[startIdx[2]]), 
		&(pgonList[2]->points[(startIdx[2] + 1) % pgonList[2]->numPoints]), 
		&(lines[2]));

	lineBisectors(&(lines[0]), &(lines[1]), &(bisect[0][0]), &(bisect[0][1]));
	lineBisectors(&(lines[0]), &(lines[2]), &(bisect[1][0]), &(bisect[1][1]));

	/*	There are four possible candidate intersection points, depending
		on which bisectors we use from each pair of lines. */

	IntersectionOf(&(bisect[0][0]), &(bisect[1][0]), &(candidate[0]));
	IntersectionOf(&(bisect[0][0]), &(bisect[1][1]), &(candidate[1]));
	IntersectionOf(&(bisect[0][1]), &(bisect[1][0]), &(candidate[2]));
	IntersectionOf(&(bisect[0][1]), &(bisect[1][1]), &(candidate[3]));

	/*	Time to choose the candidate points that keep the correct
		ccw order of the lines and that respects the distance
		constraints. */

	done = False;
	for (i = 0; (i < 4) && (done == False); i++) {
		for (j = 0; j < 3; j++) {
			ptNearestLine(&(candidate[i]), &(lines[j]), &(nearPt[j]));
		}

		if (pointsCCW(&(nearPt[0]), &(nearPt[1]), &(nearPt[2])) == True) {
			if ((nearestPointOnSegment(&(candidate[i]),
					&(pgonList[0]->points[startIdx[0]]),
					&(pgonList[0]->points[(startIdx[0] + 1) % 
						pgonList[0]->numPoints])) == True) &&
				(nearestPointOnSegment(&(candidate[i]),
					&(pgonList[1]->points[startIdx[1]]),
					&(pgonList[1]->points[(startIdx[1] + 1) % 
						pgonList[1]->numPoints])) == True) &&
				(nearestPointOnSegment(&(candidate[i]),
					&(pgonList[2]->points[startIdx[2]]),
					&(pgonList[2]->points[(startIdx[2] + 1) % 
						pgonList[2]->numPoints])) == True)) {

				PointAssignPtr(vorPoint, &(candidate[i]));
				done = True;
			}
		}
	}

	if (done) {
		middleSpoke->attachPolygon = pgonList[1];
		PointAssignPtr(&(middleSpoke->otherPt), vorPoint);
		ptNearestLine(vorPoint, &(lines[1]), &(middleSpoke->attachment));
		middleSpoke->attachIndex = 2*startIdx[1] + 1;
		getEventPoint (vorPoint, &(middleSpoke->attachment), eventPt);
	}
	else {
		Pt	tan1Start, tan1End, tan2Start, tan2End;
		int	headIdx, tailIdx;

        /*  Compute the spoke as if on the convex hull.  Set the
            event point at infinity. */

        outerCommonTangent(pgonList[0], pgonList[1], &tan1Start, &tan1End,
			&headIdx, &tailIdx);
        outerCommonTangent(pgonList[1], pgonList[2], &tan2Start, &tan2End,
			&headIdx, &tailIdx);

        ignore = objectsOnHull(&tan1Start, &tan1End, &tan2Start, &tan2End,
                vorPoint, eventPt, middleSpoke);
	}

	/*	"done" should always be returning True in this case. */
	return (done);
}


